#include "vector.h"

Vec3 vec_add(Vec3 v1, Vec3 v2)
{
    Vec3 v;
    vec_initfromxyz(&v, v1.x + v2.x, v1.y + v2.y, v1.z + v2.z);
    return v;
}

Vec3 vec_subtract(Vec3 v1, Vec3 v2)
{
    Vec3 v;
    vec_initfromxyz(&v, v1.x - v2.x, v1.y - v2.y, v1.z - v2.z);
    return v;
}

float vec_multiply(Vec3 v1, Vec3 v2)
{
    return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
}

Vec3 vec_multiplybyflt(Vec3 v, const float f)
{
    Vec3 vector;
    vec_initfromxyz(&vector, v.x * f, v.y * f, v.z * f);
    return vector;
}

Vec3 vec_divide(Vec3 v, const float f)
{
    float invf = 1.0f / f;
    Vec3 vector;
    vec_initfromxyz(&vector, v.x * invf, v.y * invf, v.z * invf);
    return vector;
}

void vec_initfromxyz(Vec3 *v, float x, float y, float z)
{
    v->x = x;
    v->y = y;
    v->z = z;
}

void vec_initfromarr(Vec3 *v, float a[3])
{
    v->x = a[0];
    v->y = a[1];
    v->z = a[2];
}

float vec_lenght2(Vec3 *v)
{
    return v->x * v->x + v->y * v->y + v->z * v->z;
}

float vec_lenght(Vec3 *v)
{
    return sqrt(vec_lenght2(v));
}

Vec3 vec_normalized(Vec3 *v)
{
    return vec_divide(*v, vec_lenght(v));
}

float vec_normalize(Vec3 *v)
{
    float len = vec_lenght(v);
    *v = vec_divide(*v, len);
    return len;
}

void vec_write(Vec3 *v, float a[3])
{
    a[0] = v->x;
    a[1] = v->y;
    a[2] = v->z;
}

void Vec3ByMatrix43(float vecOut[3], const float v[3], const float m[12])
{
    vecOut[0] = v[0] * m[0] + v[1] * m[3] + v[2] * m[6] + m[9];
    vecOut[1] = v[0] * m[1] + v[1] * m[4] + v[2] * m[7] + m[10];
    vecOut[2] = v[0] * m[2] + v[1] * m[5] + v[2] * m[8] + m[11];
}

void Norm3ByMatrix43(float vecOut[3], const float v[3], const float m[12])
{
    vecOut[0] = v[0] * m[0] + v[1] * m[3] + v[2] * m[6];
    vecOut[1] = v[0] * m[1] + v[1] * m[4] + v[2] * m[7];
    vecOut[2] = v[0] * m[2] + v[1] * m[5] + v[2] * m[8];
}

